# -*- coding: utf-8 -*-
"""
Created on Fri Jun 20 13:45:37 2014

@author: Maxim
"""
import aircraft
from FlightConditions import ISAtmosphere
import convert

import numpy as np
import matplotlib.pyplot as plt
from scipy.interpolate import interp1d


def get_hinge_derivatives(chordRatio):
    """
    calculate dCh/dalpha, dCh/delev etc.
    """
    _chordRatio = np.array([0.0, 0.064142, 0.147500, 0.273531, 0.416778, 0.609324, 0.808278, 1.0])
    _ChdDelta = np.array([0.0, -0.005036, -0.008766, -0.012467, -0.015343, -0.018889, -0.022217, -0.025265])
    curve = interp1d(_chordRatio, _ChdDelta,'cubic')
    k = _ChdDelta[-1]
    ChdDelta = curve(chordRatio)
    ChdAlpha = chordRatio*k
    return ChdDelta, ChdAlpha


def get_CLde(chordRatio):
    """ Figure 12:4 """
    _coef = np.array([0.0, 0.022441, 0.034331, 0.044059, 0.054873, 0.061538, 0.066213, 0.069563, 0.071251, 0.071868])
    _cr = np.array([0.0, 0.136181, 0.227375, 0.303276, 0.405573, 0.509740, 0.619405, 0.743567, 0.857312, 1.0])
    curve = interp1d(_cr,_coef,'cubic')
    return curve(chordRatio)


def control_force(ac,aero,velocity,altitude,mass,cgMAC,gearRatio,Vtr):
    # assumptions
    V = float(velocity)
    Ch0 = 0.0
    Dprop=1.73 # propeller diameter
    #---
    S = ac.wing.area
    St = ac.hStab.area
    be = ac.hStab.span
    ce = ac.hStab.elevator.avgChord
    TR = ac.wing.taper
    cgX = ac.wing.get_fs_on_mac(cgMAC)
    x = cgX - aero.xNP
    l = (ac.hStab.aapex[0]+0.25*ac.hStab.MAC) - (ac.wing.aapex[0]+0.25*ac.hStab.MAC)
    g = 9.81
    W = mass*g
    WS = W/S
    atm = ISAtmosphere(altitude)
    rho = atm.density
    cfc = ac.hStab.elevator.avgChordRatio
    AR = ac.wing.aspectRatio
    c = ac.wing.MAC
    xc = x/c
    ce2 = ce*ce
    Dprop = 1.
    #---
    CLa = aero.derivs.CLa # 1/rad
    CLa *= np.pi/180. # 1/deg
    CLtde = get_CLde(cfc)
    CheDdelta, CheDalpha = get_hinge_derivatives(cfc)
    it = ac.hStab.airfoil[0].polar.get_alpha_at_cl(0.0)
    #---
    deda = 20.*CLa*TR**0.3/(AR**(0.725))*(3.*c/l)**0.25
    q = rho*V*V/2.
    CLreq = W/q/S
    Treq = (aero.Cd0 + aero.k*CLreq**2.0)*q*S
    CT = Treq/(rho*V*V*Dprop)
    qtq = 1.0 + 8.*CT/np.pi
    qt = q*qtq
    #---
    de0 = WS*xc/(CLtde*qt*St*l/(q*S*c)*rho/2.*Vtr*Vtr)
    He = WS*qtq*(1.0-deda)*be*ce2*CheDalpha/CLa
    He+= WS*xc*be*ce2*CheDdelta/(St*l/c/S*CLtde)
    He+= (it+CheDalpha - de0*CheDdelta + Ch0)*qt*be*ce2
    Fe = gearRatio*He
    return convert.N_to_lbf(Fe)


def force_per_knot():
    ac = aircraft.load('V204')
    
    cgMAC     = np.array([20,28.8,37.6])
    Vtrim     = np.array([75.0, 90, 140]) #kts
    mass      = 450.0 #kg
    altitude  = 0.0   #m
    gearRatio = 1.589/0.3048 #TODO: value from FD SnC report
    #---
    lines = ['rs-','b^-','m>-','go-']
    dV = 20.0 #kts
    Vrange = np.linspace(-dV,dV,11)
    #---
    VtrimSI = np.array([convert.kt_to_msec(v) for v in Vtrim])
    VrangeSI = np.array([convert.kt_to_msec(v) for v in Vrange])
    #---
    

    Fs = np.zeros([len(cgMAC)*len(Vtrim),len(Vrange)])

    plt.figure(1)
    plt.hold(True)
    plt.grid(True)
    plt.xlabel('KEAS')
    plt.ylabel('(Push)     Elevator stick force, lbs     (Pull)')
    plt.xlim([40,170])
    plt.ylim([-6,6])
    for i,cg in enumerate(cgMAC):
        dispLegend=True
        cgX = ac.wing.get_fs_on_mac(cg)
        aero = ac.analyze_aero_trim(cg = [cgX,0,0])
        print aero.xNP, aero.SM
        for j,vtrim in enumerate(VtrimSI):
            for k,v in enumerate(VrangeSI):
                idx = i*len(cgMAC)+j
                fs = control_force(ac,aero,vtrim+v,altitude,mass,cg,gearRatio,vtrim)
                Fs[idx,k] = convert.N_to_lbf(fs)
            if dispLegend:
                plt.plot(Vtrim[j]+Vrange,Fs[idx]-Fs[idx,5],lines[i],label='CG %.1f%% MAC'%cg)
                dispLegend=False
            else:
                plt.plot(Vtrim[j]+Vrange,Fs[idx]-Fs[idx,5],lines[i])
    
    refCurve = -Vrange/6.0
    plt.plot(Vtrim[0]+Vrange,refCurve,'k-',label='1lbs / 6 kts')
    for vtrim in Vtrim[1:]:
        plt.plot(vtrim+Vrange,refCurve,'k-')
    plt.legend()
    plt.show()

if __name__=="__main__":
    force_per_knot()